#include "stm32f10x.h"                  // Device header
#include "PID.h"

tPid PidMotor1;
tPid PidMotor2;
tPid PidTrack;
tPid PidFollow;

//给结构体类型变量赋初值

void PidMotor_Init(void)
{
	PidMotor1.actual_val=0.0;
	PidMotor1.target_val=0.0;
	PidMotor1.err=0.0;
	PidMotor1.err_last=0.0;
	PidMotor1.err_sum=0.0;
	PidMotor1.Kp=0.16;
	PidMotor1.Ki=0.001;
	PidMotor1.Kd=0;
	
	PidMotor2.actual_val=0.0;
	PidMotor2.target_val=0.0;
	PidMotor2.err=0.0;
	PidMotor2.err_last=0.0;
	PidMotor2.err_sum=0.0;
	PidMotor2.Kp=0.16;
	PidMotor2.Ki=0.001;
	PidMotor2.Kd=0;
}

void PidTrack_Init(void)
{
	PidTrack.actual_val=0.0;
	PidTrack.target_val=0.0;
	PidTrack.err=0.0;
	PidTrack.err_last=0.0;
	PidTrack.err_sum=0.0;
	PidTrack.Kp=0;
	PidTrack.Ki=0;
	PidTrack.Kd=0;
}

void PidFollow_Init(void)
{
	PidFollow.actual_val=0.0;
	PidFollow.target_val=250;
	PidFollow.err=0.0;
	PidFollow.err_last=0.0;
	PidFollow.err_sum=0.0;
	PidFollow.Kp=-0.5;
	PidFollow.Ki=0;
	PidFollow.Kd=0;
}

//比例p调节控制函数
float P_realize(tPid * pid,float actual_val)
{
	pid->actual_val = actual_val;//传递真实值
	pid->err = pid->target_val - pid->actual_val;//当前误差=目标值-真实值
	//比例控制调节 输出=Kp*当前误差
	pid->actual_val = pid->Kp*pid->err;
	return pid->actual_val;
}

//比例P 积分I 控制函数
float PI_realize(tPid * pid,float actual_val)
{
	pid->actual_val = actual_val;//传递真实值
	pid->err = pid->target_val - pid->actual_val;//当前误差=目标值-真实值
	pid->err_sum += pid->err;//误差累计值 = 当前误差累计和
	//使用PI控制 输出=Kp*当前误差+Ki*误差累计值
		pid->actual_val = pid->Kp*pid->err + pid->Ki*pid->err_sum;
	return pid->actual_val;
}

// PID控制函数
float PID_realize(tPid * pid,float actual_val)
{
	pid->actual_val = actual_val;//传递真实值
	pid->err = pid->target_val - pid->actual_val;////当前误差=目标值-真实值
	pid->err_sum += pid->err;//误差累计值 = 当前误差累计和
	//使用PID控制 输出 = Kp*当前误差 + Ki*误差累计值 + Kd*(当前误差-上次误差)
	pid->actual_val = pid->Kp*pid->err + pid->Ki*pid->err_sum + pid->Kd*(pid->err - pid->err_last);
	//保存上次误差: 这次误差赋值给上次误差
	pid->err_last = pid->err;
	return pid->actual_val;
}
